/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "bbPlotUtil.h"

bbPlotUtil::bbPlotUtil(CBetBdtLocalizer *glocalizer){
	int Nx,Ny;
	glocalizer->grid->getDim(Nx,Ny);
	float reso = glocalizer->grid->getResolution();
	Grid::TCoord2D Orig =  glocalizer->grid->getWorldPos();
	
	O = new TOccupancyGrid(Nx,Ny,reso,Orig);
	float *ptrO = O->getDataPtr();
	float *ptrG = glocalizer->grid->getDataPtr();
	rgb = new RGBUtil::RGBImage(Nx,Ny);
	
	for(int i=0;i<Nx*Ny;i++){
		ptrO[i]=ptrG[i];
	}
	
	rgb->set(O->getDataPtr(), 255, 255, 255);
}


bbPlotUtil::~bbPlotUtil(){
	delete O;
	delete rgb;
}

void bbPlotUtil::save(const char *fname){
		CJPEGEncoder encoder;
		int Nx,Ny;
		O->getDim(Nx,Ny);
		encoder.SetRGBDataPtr((const char *)rgb->getDataPtr(),Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
}

/**
* Sets/resets the map to RGB
*/
void bbPlotUtil::setMap(){
	rgb->set(O->getDataPtr(), 255, 255, 255);
}

void bbPlotUtil::setMeasurement(CEnvironmentMeasurement2D &meas, Grid::pose &pos, 
																unsigned char r,
																unsigned char g,
																unsigned char b)
{
	float x,y;
	float costheta = cos(pos.a);
	float sintheta = sin(pos.a);
	
	for(int i=0;i<meas.size();i++){
		x = pos.x + meas.points[i].x * costheta - meas.points[i].y * sintheta;
		y = pos.y + meas.points[i].x * sintheta + meas.points[i].y * costheta;
		
		Grid::TPixel pix = O->toPixel(Grid::TCoord2D(x,y));
		rgb->setPixel(pix.x,pix.y, r,g, b );
	}

}
void bbPlotUtil::setMeasurement(CEnvironmentMeasurement2D &meas, Grid::pose &pos, float resolution,
																unsigned char r,
																unsigned char g,
																unsigned char b)
{
	float x,y;
	float costheta = cos(pos.a);
	float sintheta = sin(pos.a);
	rgb->setColor(r,g,b);
	
	for(int i=0;i<meas.size();i++){
		x = pos.x + meas.points[i].x * costheta - meas.points[i].y * sintheta;
		y = pos.y + meas.points[i].x * sintheta + meas.points[i].y * costheta;
		
		Grid::TPixel pix = O->toPixel(Grid::TCoord2D(x,y));
		rgb->setCircle(pix.x, pix.y, 0.1, resolution);
	}
	Grid::TPixel pix = O->toPixel(Grid::TCoord2D(pos.x,pos.y));
	rgb->setCoordinateFrame(pix.x, pix.y, pos.a, 2.0, resolution);
	rgb->setFilledCircle(pix.x, pix.y, 0.2, resolution);
}



void bbPlotUtil::setResult(CMatchStorage &matches){
	
	for(int i=matches.storage.size()-1;i>=0;i--){
		Grid::TPixel pix = O->toPixel(Grid::TCoord2D(matches.storage[i].x,matches.storage[i].y));
		
		float fscale = (matches.storage[i].weight -  matches.storage[0].weight) /
		(matches.storage[matches.storage.size()-1].weight-matches.storage[0].weight);
		float x = fscale;
		
		float r = (x >= 3.0/8.0 & x < 5.0/8.0)*(4. * x - 3./2.)+(x >= 5./8. & x < 7./8.)+(x >= 7./8.) * (-4. * x + 9./2.);
		float g = (x >= 1./8. & x < 3./8.)*(4. * x - 1./2.)+(x >= 3./8. & x < 5./8.)+(x >= 5./8. & x < 7./8.)*(-4. * x + 7./2.);
		float b = (x < 1./8.)*(4. * x + 1./2.)+(x >= 1./8. & x < 3./8.)+(x >= 3./8. & x < 5./8.)*(-4. * x + 5./2.);
	
		rgb->setPixel(pix.x,pix.y, (uchar)(b*255),  (uchar)(g*(255)), (uchar) (r*255));
	}

}